#include "stm32h745i_discovery_ts.h"
#include <rtthread.h>

int ts_test(int argc, char* argv[])
{
    rt_uint32_t test_seconds = argc > 1 ? atoi(argv[1]) : 30;

    TS_Init_t ts_init = {0};
    ts_init.Width = TS_MAX_WIDTH;
    ts_init.Height = TS_MAX_HEIGHT;
    ts_init.Orientation = TS_SWAP_XY;
    ts_init.Accuracy = 1;

    int32_t status = 0;
    status = BSP_TS_Init(0, &ts_init);
    if (status != BSP_ERROR_NONE) {
        rt_kprintf("%s: BSP_TS_Init failed!\n", __func__);
        return -1;
    }

    struct rt_device_graphic_info gdi = {0};
    rt_device_t lcd = rt_device_find("lcd");
    if (lcd) {
        rt_kprintf("%s: find device OK!\n", __func__);
        rt_device_control(lcd, RTGRAPHIC_CTRL_GET_INFO, &gdi);
        if (gdi.framebuffer) {
            rt_memset(gdi.framebuffer, 0x00, gdi.smem_len);
        }
    }

    rt_uint32_t bytes_per_pixel = (gdi.bits_per_pixel / 8);
    rt_uint32_t box_width = 4;
    rt_uint32_t x0 = 0, y0 = 0;
    rt_uint32_t start_tick = rt_tick_get();
    rt_uint32_t stop_tick = start_tick + test_seconds * RT_TICK_PER_SECOND;

    rt_kprintf("%s: test touch sensor in %ds ...\n", __func__, test_seconds);
    while (rt_tick_get() < stop_tick) {
        TS_State_t state = {0};
        status = BSP_TS_GetState(0, &state);
        if (status != BSP_ERROR_NONE) {
            rt_kprintf("%s: BSP_TS_GetState failed!\n", __func__);
            break;
        }
        if (state.TouchDetected) {
            rt_kprintf("%s: TouchDetected (%d, %d)\n", __func__, state.TouchX, state.TouchY);
            if (gdi.framebuffer) {
                x0 = state.TouchX + box_width < gdi.width ? state.TouchX : gdi.width - box_width;
                y0 = state.TouchY + box_width < gdi.width ? state.TouchY : gdi.height - box_width;
                for (int y = y0; y < y0 + box_width; y++) {
                    uint32_t pixel_offset = (y * gdi.width + x0) * bytes_per_pixel;
                    rt_memset(&gdi.framebuffer[pixel_offset], 0xFF, box_width * bytes_per_pixel);
                }
                // rt_device_control(lcd, RTGRAPHIC_CTRL_RECT_UPDATE, RT_NULL);
            }
        }
    }
    rt_kprintf("%s: test done!\n", __func__);

    status = BSP_TS_DeInit(0);
    if (status != BSP_ERROR_NONE) {
        rt_kprintf("%s: BSP_TS_DeInit failed!\n", __func__);
    }

    return 0;
}
MSH_CMD_EXPORT(ts_test, touch sensor test);

#if 0
int gs_test(int argc, char* argv[])
{
    rt_uint32_t test_seconds = argc > 1 ? atoi(argv[1]) : 30;

    int32_t status = 0;
    TS_Init_t ts_init = {0};
    ts_init.Width = TS_MAX_WIDTH;
    ts_init.Height = TS_MAX_HEIGHT;
    ts_init.Orientation = TS_SWAP_XY;
    ts_init.Accuracy = 1;
    status = BSP_TS_Init(0, &ts_init);
    if (status != BSP_ERROR_NONE) {
        rt_kprintf("%s: BSP_TS_Init failed!\n", __func__);
        return -1;
    }

    TS_Gesture_Config_t gs_config = {0};
    gs_config.DistanceLeftRight = 5;
    gs_config.DistanceUpDown = 5;
    gs_config.DistanceZoom = 10;
    gs_config.OffsetLeftRight = 5;
    gs_config.OffsetUpDown = 5;
    gs_config.Radian = 15;
    status = BSP_TS_GestureConfig(0, &gs_config);

    struct rt_device_graphic_info gdi = {0};
    rt_device_t lcd = rt_device_find("lcd");
    if (lcd) {
        rt_kprintf("%s: find device OK!\n", __func__);
        rt_device_control(lcd, RTGRAPHIC_CTRL_GET_INFO, &gdi);
        if (gdi.framebuffer) {
            rt_memset(gdi.framebuffer, 0x00, gdi.smem_len);
        }
    }

    rt_uint32_t bytes_per_pixel = (gdi.bits_per_pixel / 8);
    rt_uint32_t box_width = 50;
    int x0 = 0, y0 = 0;
    if (gdi.framebuffer) {
        x0 = gdi.width / 2 - box_width / 2;
        y0 = gdi.height / 2 - box_width / 2;
        for (int y = y0; y < y0 + box_width; y++) {
            uint32_t pixel_offset = (y * gdi.width + x0) * bytes_per_pixel;
            rt_memset(&gdi.framebuffer[pixel_offset], 0xFF, box_width * bytes_per_pixel);
        }
        // rt_device_control(lcd, RTGRAPHIC_CTRL_RECT_UPDATE, RT_NULL);
    }

    rt_uint32_t start_tick = rt_tick_get();
    rt_uint32_t stop_tick = start_tick + test_seconds * RT_TICK_PER_SECOND;

    rt_kprintf("%s: test touch sensor in %ds ...\n", __func__, test_seconds);
    while (rt_tick_get() < stop_tick) {
        TS_State_t state = {0};
        status = BSP_TS_GetState(0, &state);
        if (status != BSP_ERROR_NONE) {
            rt_kprintf("%s: BSP_TS_GetState failed!\n", __func__);
            break;
        }

        uint32_t gesture = GESTURE_ID_NO_GESTURE;
        status = BSP_TS_GetGestureId(0, &gesture);
        if (status != BSP_ERROR_NONE) {
            rt_kprintf("%s: BSP_TS_GetState failed!\n", __func__);
            break;
        }
        if (gesture != GESTURE_ID_NO_GESTURE) {
            int move = box_width / 2;
            rt_kprintf("%s: gesture %x ", __func__, gesture);
            if (gdi.framebuffer) {
                switch (gesture)
                {
                case GESTURE_ID_MOVE_UP:
                    y0 -= move;
                    rt_kprintf("MOVE_UP\n");
                    break;
                case GESTURE_ID_MOVE_DOWN:
                    y0 += move;
                    rt_kprintf("MOVE_DONE\n");
                    break;
                case GESTURE_ID_MOVE_LEFT:
                    x0 -= move;
                    rt_kprintf("MOVE_LEFT\n");
                    break;
                case GESTURE_ID_MOVE_RIGHT:
                    x0 += move;
                    rt_kprintf("MOVE_RIGHT\n");
                    break;
                case GESTURE_ID_ZOOM_IN:
                    box_width += move;
                    rt_kprintf("ZOOM_IN\n");
                    break;
                case GESTURE_ID_ZOOM_OUT:
                    box_width += move;
                    rt_kprintf("ZOOM_OUT\n");
                    break;
                default:
                    rt_kprintf("unknow\n");
                    break;
                }

                if (x0 > gdi.width - box_width) {
                    x0 = gdi.width - box_width;
                } else if (x0 < 0) {
                    x0 = 0;
                }

                if (y0 > gdi.height - box_width) {
                    y0 = gdi.height - box_width;
                } else if (y0 < 0) {
                    y0 = 0;
                }
                for (int y = y0; y < y0 + box_width; y++) {
                    uint32_t pixel_offset = (y * gdi.width + x0) * bytes_per_pixel;
                    rt_memset(&gdi.framebuffer[pixel_offset], 0xFF, box_width * bytes_per_pixel);
                }
                // rt_device_control(lcd, RTGRAPHIC_CTRL_RECT_UPDATE, RT_NULL);
            }
        }
    }
    rt_kprintf("%s: test done!\n", __func__);

    status = BSP_TS_DeInit(0);
    if (status != BSP_ERROR_NONE) {
        rt_kprintf("%s: BSP_TS_DeInit failed!\n", __func__);
    }

    return 0;
}
MSH_CMD_EXPORT(gs_test, gesture test);
#endif
